//----------------------------------------------------------------------------------------------
//	Filename:	LineWorldShapeFactory.h
//	Author:		Sandro Spina
//	Date:		14/08/2012
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#pragma once

#include <iostream>
#include <fstream>

#include <boost/shared_ptr.hpp>
#include "Shape/VertexFormats.h"
#include "Shape/PointCloud.h"
#include <nanoflann.hpp>

#include "Shape/TriangleMesh.h"
#include "Shape/KDTreeMesh.h"
#include "Scene/Scene.h"
#include "Scene/GeometricPrimitive.h"
#include "Scene/WavefrontSceneLoader.h"
#include "Maths/MonteCarlo.h"

#include <Renderer\ScanRenderer.h>
#include <Integrator\DepthIntegrator.h>
#include <Device\NullDevice.h>
#include <Device\BufferedImageDevice.h>
#include <Sampler\RandomSampler.h>
#include <Maths\Montecarlo.h>
#include <Scene\Environment.h>

namespace Illumina
{
	namespace Core
	{		
		
		class PlanarWorldShapeForge
		{
		public:						
			PlanarWorldShapeForge()
			{
				IPointCloud::ResetMaterialsIds();
			}			

			static bool CreateSurfacePoints(const Vector3 &p_centre, const Vector3 &p_normal, const Vector3 &p_up, float p_length, float p_width,  float p_thickness, 
															std::vector<CloudPoint> *p_pointList, float p_min_dist, float p_max_dist, float p_radius, float p_scale)
			{		
				OrthonormalBasis b;
				b.InitFromWU(p_normal, p_up);

				Vector3 top_left_corner = (p_centre - (b.U * p_width/2) + (b.V * p_length/2)) * p_scale;
				Vector3 top_right_corner = (p_centre + (b.U * p_width/2) + (b.V * p_length/2)) * p_scale;
				Vector3 bottom_left_corner = (p_centre - (b.U * p_width/2) - (b.V * p_length/2)) * p_scale;
				Vector3 bottom_right_corner = (p_centre + (b.U * p_width/2) - (b.V * p_length/2)) * p_scale;

				//std::cout << top_left_corner.ToString() << top_right_corner.ToString() << bottom_left_corner.ToString() << bottom_right_corner.ToString() << std::endl;

				Illumina::Core::List<Vector3> tris;

				if (p_thickness == 0)
				{					
					tris.PushBack(bottom_left_corner);
					tris.PushBack(top_left_corner);					
					tris.PushBack(top_right_corner);
					tris.PushBack(bottom_left_corner);					
					tris.PushBack(top_right_corner);
					tris.PushBack(bottom_right_corner);
					GeneratePointsFromTrisList(tris, *p_pointList, p_min_dist, p_max_dist, p_radius, 1.f, 1.f, 1.f, IPointCloud::GetNextMaterialId());				
				}
				else 
				{
					//We need to generate 6 planes ...
				}				

				return true;
			}

			static bool CreateSurfaceTris(const Vector3 &p_centre, const Vector3 &p_normal, const Vector3 &p_up, float p_length, float p_width,  float p_thickness, 
															std::vector<Vector3> *p_trisList, float p_min_dist, float p_max_dist, float p_radius, float p_scale)
			{		
				OrthonormalBasis b;
				b.InitFromWU(p_normal, p_up);

				Vector3 top_left_corner = (p_centre - (b.U * p_width/2) + (b.V * p_length/2)) * p_scale;
				Vector3 top_right_corner = (p_centre + (b.U * p_width/2) + (b.V * p_length/2)) * p_scale;
				Vector3 bottom_left_corner = (p_centre - (b.U * p_width/2) - (b.V * p_length/2)) * p_scale;
				Vector3 bottom_right_corner = (p_centre + (b.U * p_width/2) - (b.V * p_length/2)) * p_scale;

				//std::cout << top_left_corner.ToString() << top_right_corner.ToString() << bottom_left_corner.ToString() << bottom_right_corner.ToString() << std::endl;				

				if (p_thickness == 0)
				{					
					p_trisList->push_back(bottom_left_corner);
					p_trisList->push_back(top_left_corner);					
					p_trisList->push_back(top_right_corner);
					p_trisList->push_back(bottom_left_corner);					
					p_trisList->push_back(top_right_corner);
					p_trisList->push_back(bottom_right_corner);					
				}
				else 
				{
					//We need to generate 6 planes ...
				}				

				return true;
			}			

			static void CreateCornellBox(const Vector3 &p_centre, float p_length, float p_width, float p_height, float p_thickness, 
															std::vector<CloudPoint> *p_pointList, float p_min_dist, float p_max_dist, float p_radius, float p_scale)
			{
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(0,p_height/2,0), Vector3::UnitYPos, Vector3::UnitXPos, p_length, p_width, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(0,-p_height/2,0), Vector3::UnitYPos, Vector3::UnitXPos, p_length, p_width, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_width/2,0,0), Vector3::UnitXPos, Vector3::UnitYPos, p_height, p_length, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(-p_width/2,0,0), Vector3::UnitXPos, Vector3::UnitYPos, p_height, p_length, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(0,0,p_length/2), Vector3::UnitZPos, Vector3::UnitXPos, p_height, p_width, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
			}

			static void CreateBox(const Vector3 &p_centre, Vector3 &p_W, Vector3 &p_U, Vector3 &p_V, 
															float p_length, float p_width, float p_height, float p_thickness, 
															std::vector<CloudPoint> *p_pointList, float p_minDist, float p_maxDist, float p_radius, float p_scale)
			{
				p_U.Normalize(); p_V.Normalize(); p_W.Normalize();
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre - (p_W * p_height/2)), p_W, p_V, p_length, p_width, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre + (p_W * p_height/2)), p_W, p_V, p_length, p_width, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre + (p_U * p_length/2)), p_U, p_V, p_height, p_length, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre - (p_U * p_length/2)), p_U, p_V, p_height, p_length, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre + (p_V * p_width/2)), p_V, p_U, p_height, p_width, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
				PlanarWorldShapeForge::CreateSurfacePoints(Vector3(p_centre - (p_V * p_width/2)), p_V, p_U, p_height, p_width, 0, p_pointList, p_minDist, p_maxDist, p_radius, p_scale);
			}

			//p_v0 represent the base of the stairs. Creates a stairs with an object (table or chair for now) on each horizontal surface
			static bool CreateStairs(Vector3 &p_v0, const Vector3 &p_stepForwardDir, const Vector3 &p_stepUpDir, 
									float p_stepLength, float p_stepWidth, float p_stepHeight, 
									int p_numberOfSteps, std::vector<CloudPoint> *p_pointList, float p_min_dist, float p_max_dist, float p_radius, float p_scale)
			{
				Vector3 stepNormal1(p_stepForwardDir);
				Vector3 stepNormal2(p_stepUpDir);

				stepNormal1.Normalize();
				stepNormal2.Normalize();

				//p_stepForwardDir.Normalize();
				//p_stepUpDir.Normalize();

				Vector3 nxtOrigin(p_v0.X, p_v0.Y, p_v0.Z);				
				for (int i=0; i<p_numberOfSteps; i++)
				{
					//one step = vertical + horizontal; Starting from p_v0.
					p_v0.X = nxtOrigin.X; p_v0.Y = nxtOrigin.Y; p_v0.Z = nxtOrigin.Z;
					Vector3::Add(p_v0, Vector3((stepNormal1.X) * p_stepWidth/2, (stepNormal1.Y) * p_stepWidth/2, (stepNormal1.Z) * p_stepWidth/2), nxtOrigin);
					p_v0.X = nxtOrigin.X; p_v0.Y = nxtOrigin.Y; p_v0.Z = nxtOrigin.Z;
					Vector3::Add(p_v0, Vector3((stepNormal2.X) * p_stepHeight/2, (stepNormal2.Y) * p_stepHeight/2, (stepNormal2.Z) * p_stepHeight/2), nxtOrigin);					
					PlanarWorldShapeForge::CreateSurfacePoints(nxtOrigin, stepNormal1, stepNormal2, p_stepLength, p_stepHeight, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
					
					p_v0.X = nxtOrigin.X; p_v0.Y = nxtOrigin.Y; p_v0.Z = nxtOrigin.Z;
					Vector3::Add(p_v0, Vector3((stepNormal1.X) * p_stepWidth/2, (stepNormal1.Y) * p_stepWidth/2, (stepNormal1.Z) * p_stepWidth/2), nxtOrigin);
					p_v0.X = nxtOrigin.X; p_v0.Y = nxtOrigin.Y; p_v0.Z = nxtOrigin.Z;
					Vector3::Add(p_v0, Vector3((stepNormal2.X) * p_stepHeight/2, (stepNormal2.Y) * p_stepHeight/2, (stepNormal2.Z) * p_stepHeight/2), nxtOrigin);					
					PlanarWorldShapeForge::CreateSurfacePoints(nxtOrigin, stepNormal2, stepNormal1, p_stepLength, p_stepWidth, 0, p_pointList, p_min_dist, p_max_dist, p_radius, p_scale);
				}
				return true;
			}

			static bool CreateChair(Vector3 &p_base, const Vector3 &p_UpDir, const Vector3 &p_ForwardDir, 
									float p_legsLength, float p_seatWidth, float p_seatLength, 
									float p_backLength, std::vector<CloudPoint> *p_pointList, float p_density, float p_radius)
			{

			}
			
			static bool CreateFromOBJFile(const std::string &p_filepath, Environment * p_pEnvironment, std::vector<CloudPoint> *p_pointList, float p_min_dist, float p_max_dist, float p_radius, float p_scale)
			{
				boost::timer timer;
				std::cout << "[PWSF] Loading OBJ ... ";
				WavefrontSceneLoader a(p_pEnvironment);
				KDTreeMesh * mesh = (KDTreeMesh*) a.GetTriangleMesh(p_filepath);
				mesh->ComputeBoundingVolume();
				mesh->Compile();

				float min, max;				
				mesh->ComputeRadii(min, max);

				float mesh_area = mesh->GetArea();
				IBoundingVolume * bounding_vol = mesh->GetBoundingVolume();
				
				std::cout << "Bounding vol = " << bounding_vol->GetSize().ToString() << " " << std::endl;
				float avg_tri_area = mesh_area / mesh->TriangleList.Size();
				//std::cout << mesh->TriangleList.Size() << " " << std::endl;
				List<Vector3> tris;
				int trisCount = mesh->TriangleList.Size();
				for (int i=0; i<trisCount; i++)
				{
					tris.PushBack(Vector3(mesh->VertexList[mesh->TriangleList[i].GetVertexIndex(0)].Position * p_scale));
					tris.PushBack(Vector3(mesh->VertexList[mesh->TriangleList[i].GetVertexIndex(1)].Position * p_scale));
					tris.PushBack(Vector3(mesh->VertexList[mesh->TriangleList[i].GetVertexIndex(2)].Position * p_scale));
				}
				//std::cout << " avg tri area = " << avg_tri_area << " [" << timer.elapsed() << "]" << std::endl;
				std::cout << "[PWSF] Generating points from " << tris.Size()/3 << " Tris. " << " Min_radius=" << min << " max_radius=" << max << std::endl;
				timer.restart();
				GeneratePointsFromTrisList(mesh, tris, *p_pointList, p_min_dist, p_max_dist, p_radius, avg_tri_area, min, max, IPointCloud::GetNextMaterialId());
				std::cout << "[PWSF] Generated " << p_pointList->size() << " points from " << tris.Size()/3 << " tris." << " [" << timer.elapsed() << "]" << std:: endl;				
				delete mesh;
				return true;
			}						

			static bool CreateUsingScannerFromOBJFile(const std::string &p_filepath, Environment * p_pEnvironment, std::vector<CloudPoint> *p_pointList, float p_min_dist, float p_max_dist, float p_radius, float p_scale, int p_scans)
			{
				boost::timer timer;
				std::cout << "[PWSF] Loading OBJ for Scanning ... ";
				WavefrontSceneLoader a(p_pEnvironment);
				KDTreeMesh * mesh = (KDTreeMesh*) a.GetTriangleMesh(p_filepath);
				mesh->ComputeBoundingVolume();
				mesh->Compile();

				float mext0 = mesh->GetBoundingVolume()->GetExtent().Length();
				float scale = p_pEnvironment->GetScalingFactor() / mext0; // 6 will be parametrised					
				Vector3 translate = mesh->GetBoundingVolume()->GetCentre();
				
				for (int i = 0; i<mesh->VertexList.Size(); i++)	 { mesh->VertexList[i].Position -= translate; mesh->VertexList[i].Position *= scale; } 					
				for (int i = 0; i<mesh->TriangleList.Size(); i++) { mesh->TriangleList[i].ComputeEdges(); }								
				mesh->ComputeBoundingVolume();										
				mesh->Compile();

				GeometricPrimitive * pointcloud_geometry = (GeometricPrimitive*)(p_pEnvironment->GetScene()->GetSpace()->PrimitiveList[0]);
				pointcloud_geometry->SetShape(mesh);

				IBoundingVolume * bounding_vol = mesh->GetBoundingVolume();								
				std::cout << "[PWSF] Scene Bounding Volume = " << bounding_vol->GetSize().ToString() << " " << std::endl;
												
				timer.restart();				
				DepthIntegrator * depth_integrator = (DepthIntegrator*)(p_pEnvironment->GetEngineKernel()->GetIntegratorManager()->RequestInstance("Integrator"));
				NullDevice * null_device = new NullDevice(1920, 1920);		
				BufferedImageDevice * image_device = (BufferedImageDevice*) (p_pEnvironment->GetEngineKernel()->GetDeviceManager()->RequestInstance("BIDevice"));
				std::vector<std::vector<Vector3>> scans;	
				ScanModel(p_scans, "scene", scans, depth_integrator, image_device, p_pEnvironment);
				for (int i=0; i<scans.size(); i++) 					
					for (int j=0; j<scans[i].size(); j++) 
						p_pointList->push_back(CloudPoint(scans[i][j]));	
				
				std::cout << "[PWSF] Scanner Generated " << p_pointList->size() << " points." << " [" << timer.elapsed() << "]" << std:: endl;				
				delete mesh;
				return true;
			}

			static void ScanModel(int p_views, std::string p_model_name, std::vector<std::vector<Vector3>> &p_scans, DepthIntegrator *p_integrator, BufferedImageDevice *p_imageDevice, Environment * p_environment)
			{
				RandomSampler rs;				
				std::vector<Vector3> scan_points_per_view;
				std::string file_output_path;
				RadianceBuffer rbuffer(p_imageDevice->GetWidth(), p_imageDevice->GetHeight());
				ScanRenderer renderer(p_environment->GetScene(), p_integrator, p_imageDevice, p_environment->GetEngineKernel()->GetFilterManager()->RequestInstance("Filter"), &rbuffer, &scan_points_per_view, 1);		
				Vector2 sample;
				Vector3 camera_position;
				for (int i = 0; i<p_views; i++)
				{
					sample = rs.Get2DSample();			
					camera_position = Vector3::Zero + 20.f * Montecarlo::UniformSampleSphere(sample.X, sample.Y);			
					renderer.GetScene()->GetCamera()->MoveTo(camera_position);
					renderer.GetScene()->GetCamera()->LookAt(Vector3::Zero);					
					scan_points_per_view.clear();
					renderer.Render();
					renderer.Commit();		
					file_output_path = "Output/PointClouds/" + p_model_name +  "_view" + boost::lexical_cast<std::string, int>(i) + ".asc";					
					p_scans.push_back(scan_points_per_view);
				}
			}

			/* density is a value between 0 (sparsest) and 1 (densest) point generation
			 *
			 *
			 */ 
			static void GeneratePointsOnPlane(Vector3 &p_00, Vector3 &p_01, Vector3 &p_10, List<CloudPoint> &p_pointList, float p_density, float p_radius, int p_materialId) 
			{		
				float dist_00_01 = Vector3::Distance(p_00, p_01);
				float dist_10_11 = Vector3::Distance(p_00, p_10);
				int max_points_0 = dist_00_01 / (p_radius*2);
				int max_points_1 = dist_10_11 / (p_radius*2);
				int num_points_0 = (max_points_0 * p_density) +1; //there's always at least one point
				int num_points_1 = (max_points_1 * p_density) +1; //there's always at least one point

				float dist_between_points_0 = dist_00_01 / num_points_0;
				float dist_between_points_1 = dist_10_11 / num_points_1;

				Vector3 direction_0 = Vector3::Normalize(p_01 - p_00);
				Vector3 direction_1 = Vector3::Normalize(p_10 - p_00);
				Vector3 start_pos;

				for (int x=0; x <=num_points_0; x++)
				{					
					start_pos = p_00 + (direction_0 * x * dist_between_points_0);
					for (int y=0; y<=num_points_1; y++)
					{
						VertexP next_point;
						next_point.Position = start_pos + (direction_1*y*dist_between_points_1);					
						p_pointList.PushBack(CloudPoint(next_point, p_materialId, 0, -1));
					}
				}
			}

			/* density is a value between 0 (sparsest) and 1 (densest) point generation
			 *
			 *
			 */ 
			static void GeneratePointsFromTrisList(List<Vector3> tris, std::vector<CloudPoint> &p_pointList, float p_density_min, float p_density_max, float p_radius, float p_avg_area, float p_min_area, float p_max_area, int p_materialId) 
			{		
				std::vector<Vector3> pointsOnEdges;					
				std::vector<Vector3> prevScanLine;

				Vector3 v0, v1, v2, vv0, vv1, vvCommon, vvCross;
				float dist_v0_v1, dist_v0_v2, dist_v1_v2;
				float area, n_area, local_density;

				Vector3 vv0Dir, vv1Dir;

				float dist_vvCommon_vv0, dist_vvCommon_vv1, dist_vv0_vv1;
					
				int max_points_vv0, max_points_vv1, num_points_vv0, num_points_vv1;

				float dist_between_points_vv0, dist_between_points_vv1;

				Vector3 direction_0, direction_1, start_pos;
				float step_size_to_vCommon, step_size_to_vOther;
				int number_of_points;

				float interpolated_dist_vvCommon_vvOther;
				int num_points_vv0_vv1, max_points_vv0_vv1;
				float dist_between_points_vv0_vv1;
				Vector3 direction_vv0_vv1;
				bool add_start, add_end;	

				Vector3 interpolatedStartPos, interpolatedEndPos;
				VertexP next_point;

				float max_edge = Maths::Sqrt(p_max_area);
				float min_edge = Maths::Sqrt(p_min_area);

				for (int i = 0; i<tris.Size(); )
				{
					if (i!=0 && i % 10000==0)	std::cout << "[PWSF] " << "(" << i/3 << ")"  <<  " Points Created = " << p_pointList.size() << std::endl; 
					//Get Triangle Vertices
					v0 = tris.At(i); i++;
					v1 = tris.At(i); i++;
					v2 = tris.At(i); i++;					

					dist_v0_v1 = Vector3::DistanceSquared(v0, v1);
					dist_v1_v2 = Vector3::DistanceSquared(v1, v2);
					dist_v0_v2 = Vector3::DistanceSquared(v0, v2);

					vv0;
					vv1;
					vvCommon;

					if ((dist_v0_v1 >= dist_v1_v2) & (dist_v0_v1 >= dist_v0_v2))
					{
						vv0 = v0;
						vv1 = v1;
						vvCommon = v2;
					} 
					else
					{
						if ((dist_v0_v2 >= dist_v0_v1) & (dist_v0_v2 >= dist_v1_v2))
						{
							vv0 = v0;
							vv1 = v2;
							vvCommon = v1;
						} 
						else
						{
							if ((dist_v1_v2 >= dist_v0_v1) & (dist_v1_v2 >= dist_v0_v2))
							{
								vv0 = v1;
								vv1 = v2;
								vvCommon = v0;
							}
						}
					}
					
					dist_vvCommon_vv0 = Vector3::Distance(vvCommon, vv0);
					dist_vvCommon_vv1 = Vector3::Distance(vvCommon, vv1);
					dist_vv0_vv1 = Vector3::Distance(vv0, vv1);					
					Vector3::Cross(vv0-vvCommon, vv1-vvCommon, vvCross);
					area = (vvCross.Length() / 2); 

					Vector3 centre = (vv0 + vv1 + vvCommon) / 3;
					float radius_sq = Vector3::DistanceSquared(centre, vvCommon);
					if (Vector3::DistanceSquared(centre, vv0) > radius_sq) radius_sq = Vector3::DistanceSquared(centre, vv0);
					if (Vector3::DistanceSquared(centre, vv1) > radius_sq) radius_sq = Vector3::DistanceSquared(centre, vv1);

					radius_sq = Maths::Sqrt(radius_sq);					
					radius_sq = Maths::Min((radius_sq - p_min_area) / (p_max_area - p_min_area), 1);										
					local_density = ((((Maths::Exp(2*radius_sq)-1) / (Maths::Exp(2)-1))) * (p_density_max - p_density_min)) + p_density_min;

					//std::cout << "radius_sq = " << radius_sq << " density = " << local_density << std::endl;
					
					num_points_vv0 = dist_vvCommon_vv0 / local_density; if (num_points_vv0==0) num_points_vv0 = 1; //there's always at least one point
					num_points_vv1 = dist_vvCommon_vv1 / local_density; if (num_points_vv1==0) num_points_vv1 = 1; //there's always at least one point

					dist_between_points_vv0 = dist_vvCommon_vv0 / num_points_vv0;
					dist_between_points_vv1 = dist_vvCommon_vv1 / num_points_vv1;					

					if (dist_between_points_vv0 < dist_between_points_vv1)
					{
						Vector3::Normalize(vvCommon - vv0, direction_0);
						Vector3::Normalize(vv1 - vv0, direction_1);
						start_pos = vv0;
						step_size_to_vCommon = dist_between_points_vv0;
						step_size_to_vOther = dist_vv0_vv1 / num_points_vv0;
						number_of_points = num_points_vv0;
					}
					else
					{
						Vector3::Normalize(vvCommon - vv1, direction_0);
						Vector3::Normalize(vv0 - vv1, direction_1);
						start_pos = vv1;
						step_size_to_vCommon = dist_between_points_vv1;
						step_size_to_vOther = dist_vv0_vv1 / num_points_vv1;
						number_of_points = num_points_vv1;
					}														

					//std::cout << "v0 " << v0.ToString() << " v1 " << v1.ToString() << " v2 " << v2.ToString() << std::endl;					
					//std::cout << "vCommon " << vvCommon.ToString() << " vv0 " << vv0.ToString() << " vv1 " << vv1.ToString() << std::endl;
					//std::cout << "Dst[v0-v1]=" << dist_v0_v1 << " Dst[v0-v2]=" << dist_v0_v2 << " Dst[v1-v2]=" << dist_v1_v2 << "\n" << std::endl;
					//std::cout << "LERP from " << start_pos.ToString() << " to " << direction_0.ToString() << " and " << direction_1.ToString() << std::endl;

					add_start = true;
					add_end = true;	
					int points_generated = 0;

					Ray v_ray;
					Vector3 v_ray_direction;
					bool visible = false;

					for (int x=0; x<=number_of_points; x++)
					{					
						interpolatedStartPos = start_pos + (direction_0 * x * step_size_to_vCommon);
						interpolatedEndPos = start_pos + (direction_1 * x * step_size_to_vOther);
						Vector3::Normalize(interpolatedEndPos - interpolatedStartPos, direction_vv0_vv1);
						interpolated_dist_vvCommon_vvOther = Vector3::Distance(interpolatedStartPos, interpolatedEndPos);
						if (interpolated_dist_vvCommon_vvOther < 0.0001f) continue;
						num_points_vv0_vv1 = interpolated_dist_vvCommon_vvOther / local_density;
						if (num_points_vv0_vv1 == 0) num_points_vv0_vv1 = 1;
						dist_between_points_vv0_vv1 = interpolated_dist_vvCommon_vvOther / num_points_vv0_vv1;

						add_start = true;
						add_end = true;						
												
						for (int y=0; y<=num_points_vv0_vv1; y++)
						{							
							next_point.Position = interpolatedStartPos + (direction_vv0_vv1*y*dist_between_points_vv0_vv1);												
							p_pointList.push_back(CloudPoint(next_point, p_materialId, 0, -1));
							points_generated++;							
						}						
					}
					//std::cout << "Points generated = " << points_generated << " points on Edge = " << pointsOnEdges.size() << std::endl;
				}
			}

			/* density is a value between 0 (sparsest) and 1 (densest) point generation
			 *
			 *
			 */ 
			static void GeneratePointsFromTrisList(KDTreeMesh * p_mesh, List<Vector3> tris, std::vector<CloudPoint> &p_pointList, float p_density_min, float p_density_max, float p_radius, float p_avg_area, float p_min_area, float p_max_area, int p_materialId) 
			{		
				std::vector<Vector3> pointsOnEdges;					
				std::vector<Vector3> prevScanLine;

				Vector3 v0, v1, v2, vv0, vv1, vvCommon, vvCross;
				float dist_v0_v1, dist_v0_v2, dist_v1_v2;
				float area, n_area, local_density;

				Vector3 vv0Dir, vv1Dir;

				float dist_vvCommon_vv0, dist_vvCommon_vv1, dist_vv0_vv1;
					
				int max_points_vv0, max_points_vv1, num_points_vv0, num_points_vv1;

				float dist_between_points_vv0, dist_between_points_vv1;

				Vector3 direction_0, direction_1, start_pos;
				float step_size_to_vCommon, step_size_to_vOther;
				int number_of_points = 1;

				float interpolated_dist_vvCommon_vvOther;
				int num_points_vv0_vv1, max_points_vv0_vv1;
				float dist_between_points_vv0_vv1;
				Vector3 direction_vv0_vv1;
				bool add_start, add_end;	

				Vector3 interpolatedStartPos, interpolatedEndPos;
				VertexP next_point;

				float max_edge = Maths::Sqrt(p_max_area);
				float min_edge = Maths::Sqrt(p_min_area);

				int points_generated = 0;
				int points_discarded = 0;

				for (int i = 0; i<tris.Size(); )
				{
					if (i!=0 && i % 10000==0)	std::cout << "[PWSF] " << "(" << i/3 << ")"  <<  " Points Created = " << p_pointList.size() << std::endl; 
					//Get Triangle Vertices
					v0 = tris.At(i); i++;
					v1 = tris.At(i); i++;
					v2 = tris.At(i); i++;					

					dist_v0_v1 = Vector3::DistanceSquared(v0, v1);
					dist_v1_v2 = Vector3::DistanceSquared(v1, v2);
					dist_v0_v2 = Vector3::DistanceSquared(v0, v2);

					vv0;
					vv1;
					vvCommon;

					if ((dist_v0_v1 >= dist_v1_v2) & (dist_v0_v1 >= dist_v0_v2))
					{
						vv0 = v0;
						vv1 = v1;
						vvCommon = v2;
					} 
					else
					{
						if ((dist_v0_v2 >= dist_v0_v1) & (dist_v0_v2 >= dist_v1_v2))
						{
							vv0 = v0;
							vv1 = v2;
							vvCommon = v1;
						} 
						else
						{
							if ((dist_v1_v2 >= dist_v0_v1) & (dist_v1_v2 >= dist_v0_v2))
							{
								vv0 = v1;
								vv1 = v2;
								vvCommon = v0;
							}
						}
					}

					//std::cout << "T: " << vv0.ToString() << " " << vv1.ToString() << " " << vvCommon.ToString() << std::endl;						

					dist_vvCommon_vv0 = Vector3::Distance(vvCommon, vv0);
					dist_vvCommon_vv1 = Vector3::Distance(vvCommon, vv1);
					dist_vv0_vv1 = Vector3::Distance(vv0, vv1);					
					Vector3::Cross(vv0-vvCommon, vv1-vvCommon, vvCross);
					area = (vvCross.Length() / 2); 

					Vector3 centre = (vv0 + vv1 + vvCommon) / 3;
					float radius_sq = Vector3::DistanceSquared(centre, vvCommon);
					if (Vector3::DistanceSquared(centre, vv0) > radius_sq) radius_sq = Vector3::DistanceSquared(centre, vv0);
					if (Vector3::DistanceSquared(centre, vv1) > radius_sq) radius_sq = Vector3::DistanceSquared(centre, vv1);
				
					radius_sq = Maths::Sqrt(radius_sq);					
					if (p_min_area==p_max_area) p_max_area+=0.001f;
					radius_sq = Maths::Min((radius_sq - p_min_area) / (p_max_area - p_min_area), 1);										
					local_density = ((((Maths::Exp(2*radius_sq)-1) / (Maths::Exp(2)-1))) * (p_density_max - p_density_min)) + p_density_min;

					//std::cout << "radius_sq = " << radius_sq << " density = " << local_density << std::endl;
					
					num_points_vv0 = dist_vvCommon_vv0 / local_density; if (num_points_vv0==0) num_points_vv0 = 1; //there's always at least one point
					num_points_vv1 = dist_vvCommon_vv1 / local_density; if (num_points_vv1==0) num_points_vv1 = 1; //there's always at least one point
					
					dist_between_points_vv0 = dist_vvCommon_vv0 / num_points_vv0;
					dist_between_points_vv1 = dist_vvCommon_vv1 / num_points_vv1;					

					if (dist_between_points_vv0 < dist_between_points_vv1)
					{
						Vector3::Normalize(vvCommon - vv0, direction_0);
						Vector3::Normalize(vv1 - vv0, direction_1);
						start_pos = vv0;
						step_size_to_vCommon = dist_between_points_vv0;
						step_size_to_vOther = dist_vv0_vv1 / num_points_vv0;
						number_of_points = num_points_vv0;
					}
					else
					{
						Vector3::Normalize(vvCommon - vv1, direction_0);
						Vector3::Normalize(vv0 - vv1, direction_1);
						start_pos = vv1;
						step_size_to_vCommon = dist_between_points_vv1;
						step_size_to_vOther = dist_vv0_vv1 / num_points_vv1;
						number_of_points = num_points_vv1;
					}														

					//std::cout << "v0 " << v0.ToString() << " v1 " << v1.ToString() << " v2 " << v2.ToString() << std::endl;					
					//std::cout << "vCommon " << vvCommon.ToString() << " vv0 " << vv0.ToString() << " vv1 " << vv1.ToString() << std::endl;
					//std::cout << "Dst[v0-v1]=" << dist_v0_v1 << " Dst[v0-v2]=" << dist_v0_v2 << " Dst[v1-v2]=" << dist_v1_v2 << "\n" << std::endl;
					//std::cout << "LERP from " << start_pos.ToString() << " to " << direction_0.ToString() << " and " << direction_1.ToString() << std::endl;

					add_start = true;
					add_end = true;	
					
					Ray v_ray;
					Vector3 v_ray_direction;
					Vector3 v_ray_origin;
					v_ray.Min = -1;
					v_ray.Max = Maths::Maximum;
					bool visible = false;

					//points_generated = 0;
					//points_discarded = 0;
					
					for (int x=0; x<=number_of_points; x++)
					{					
						interpolatedStartPos = start_pos + (direction_0 * x * step_size_to_vCommon);
						interpolatedEndPos = start_pos + (direction_1 * x * step_size_to_vOther);
						Vector3::Normalize(interpolatedEndPos - interpolatedStartPos, direction_vv0_vv1);
						interpolated_dist_vvCommon_vvOther = Vector3::Distance(interpolatedStartPos, interpolatedEndPos);
						if (interpolated_dist_vvCommon_vvOther < 0.0001f) continue;
						num_points_vv0_vv1 = interpolated_dist_vvCommon_vvOther / local_density;
						if (num_points_vv0_vv1 == 0) num_points_vv0_vv1 = 1;
						dist_between_points_vv0_vv1 = interpolated_dist_vvCommon_vvOther / num_points_vv0_vv1;

						add_start = true;
						add_end = true;						
									
						for (int y=0; y<=num_points_vv0_vv1; y++)
						{							
							next_point.Position = interpolatedStartPos + (direction_vv0_vv1*y*dist_between_points_vv0_vv1);							
							visible=false;
							//Make sure this point is visible and not totally occluded by other triangles from the model
							for (float uu=0.0001f; uu<1; uu+=0.1f)
							{
								for (float vv=0.0001f; vv<1; vv+=0.1f)
								{
									v_ray_direction = Montecarlo::UniformSampleSphere(uu, vv);
									v_ray_origin = next_point.Position + (v_ray.Direction*0.0001f);
									v_ray.Set(v_ray_origin, v_ray_direction, -1.f);
									//std::cout << "Ray O=" << v_ray.Origin.ToString() << " D=" << v_ray.Direction.ToString() << std::endl;
									if (!p_mesh->Intersects(v_ray)) { visible = true; break; }
								}
								if (visible) break;
							}

							if (visible) //Add to current list
							{ 
								p_pointList.push_back(CloudPoint(next_point, p_materialId, 0, -1));
								points_generated++;
							} else { 
								// discard point
								points_discarded++;
							}
						}						
					}
					//std::cout << "Points generated = " << points_generated << " : points discarded = " << points_discarded << std::endl;
				}
				std::cout << "[PWSF] Points generated = " << points_generated << " : points discarded (not visible) = " << points_discarded << std::endl;
			}

			static void AddNoise(List<CloudPoint> &p_pointList, float p_noise)
			{

			}

		};
	}
}